import casadi as ca
def so3_log(R):
    """
    CasADi-compatible SO(3) logarithm map (with safe division).
    """
    trace_R = ca.trace(R)
    cos_theta = (trace_R - 1) / 2
    cos_theta = ca.fmin(1.0, ca.fmax(-1.0, cos_theta))
    theta = ca.acos(cos_theta)

    sin_theta = ca.sin(theta)

    # 反对称部分 (R - R.T)
    omega_hat_matrix = (R - R.T) / 2

    # 角速度向量
    omega_unscaled = ca.vertcat(
        omega_hat_matrix[2,1],
        omega_hat_matrix[0,2],
        omega_hat_matrix[1,0]
    )

    # 用 ca.if_else 防止除以零
    eps = 1e-4
    omega = ca.if_else(
        ca.fabs(sin_theta) > eps,
        theta * omega_unscaled / sin_theta,
        ca.SX.zeros(3)  # θ → 0 时，log(R) → 0
    )

    return omega